#include <PID_v1.h>
#include <Metro.h>

const byte SensPin = 5;
const byte PWMPin = 3; //Analog
const byte HoldPin = 6; //Analog
const int HoldMax = 800; //Max EinstellWert
const int HoldMin = 300; //Min EinstellWert
const int SensMax = 950; //Max Sensorwert
const int SensMin = 500; //Min Sensorwert
const int SensSamples = 20; //Anzahl Messungen (Sensor smooth)

//PID
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,0.2,0.5,0.2, REVERSE);

//SensorSmooth
int SensValArray[SensSamples];

Metro timerUpdateSense(200);
Metro timerUpdateSerial(500);

void setup()
{
  Serial.begin(9600);
  pinMode(PWMPin, OUTPUT);

  Input = analogRead(SensPin);
  for(int i=0;i<=SensSamples;i++)
    SensValArray[i]=Input;

  Setpoint = analogRead(HoldPin);

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  if (timerUpdateSense.check()) {
    int SensVal = GetSensorVal(); 
    SensVal = digitalSmooth(SensVal, SensValArray);
    Input = SensVal;
    
    SensVal = analogRead(HoldPin);
    SensVal = map(SensVal, 0, 1024, HoldMin, HoldMax);
    Setpoint = SensVal;
    
    myPID.Compute();
    
    analogWrite(PWMPin,Output);
  }

  if (timerUpdateSerial.check()) {
    Serial.print(Input);Serial.print(" Input (");Serial.print(GetSensorVal());Serial.print(") \t");
    Serial.print(Setpoint); Serial.print(" Setpoint \t");
    Serial.print(Output);  Serial.println(" Output");    
    Serial.flush();
  }

  delay(1);
}

int GetSensorVal(){
  int sensVal = analogRead(SensPin); 
  sensVal = constrain(sensVal, SensMin, SensMax);
  sensVal = map(sensVal, SensMin, SensMax, 0, 1024);
  return sensVal;  
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  unsigned long total;
  static int i;
  static int sorted[SensSamples];
  boolean done;

  i = (i + 1) % SensSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<SensSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (SensSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((SensSamples * 15)  / 100), 1); 
  top = min((((SensSamples * 85) / 100) + 1  ), (SensSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}

